#ifndef I_LIB_GEOMETRY_I_LINE_H
#define I_LIB_GEOMETRY_I_LINE_H

#include <cassert>
#include "../algorithm/i_sort.h"
#include "../numeric/i_eig.h"

namespace idl
{
    /*Compute the projection (q) of a 2D point (p) on a line (l) in 2D space
    the line l passes the origin "po" (0,0), i.e, ax+by = 0*/
    template <typename T> 
    inline void i_point_on_po_line_projection_2d(const T l[2], const T p[2], T q[2])
    {
        if (l[0] == (T)0 && l[1] == (T)0) /*x,y = (0,0)*/
        {
            q[0] = -p[0];
            q[1] = -p[1];
        }
        else if (l[0] == (T)0) /*y = 0*/
        {
            q[0] = p[0];
            q[1] = -p[1];
        }
        else if (l[1] == (T)0)/*x = 0*/
        {
            q[0] = -p[0];
            q[1] = p[1];
        }
        else
        {
            T ma[4], b[2];
            ma[0] = l[0];
            ma[1] = l[1];
            ma[2] = -l[1];
            ma[3] = l[0];
            b[0] = 0;
            b[1] = i_dot2(ma + 2, p);
            i_solve_2x2(ma, b, q);
        }
    }

    /*Compute the projection (q) of a 2D point (p) on a line (l) in 2D space: ax+by+c = 0*/
    template <typename T> inline void i_point_on_line_projection_2d(
        const T l[3], const T p[2], T q[2])
    {
        if (l[2] == (T)0)
        {
            i_point_on_po_line_projection_2d(l, p, q);
        }
        else
        {
            if (l[0] == (T)0) /*y + c/b = 0, assert b!=0*/
            {
                q[0] = p[0];
                q[1] = -2 * i_div(l[2], l[1]) - p[1];
            }
            else if (l[1] == (T)0) /*x + c/a = 0, assert a!=0*/
            {
                q[0] = -2 * i_div(l[2], l[0]) - p[0];
                q[1] = p[1];
            }
            else
            {
                T ma[4], b[2];
                ma[0] = l[0];
                ma[1] = l[1];
                ma[2] = -l[1];
                ma[3] = l[0];
                b[0] = -l[2];
                b[1] = i_dot2(ma + 2, p);
                i_solve_2x2(ma, b, q);
            }
        }
    }

    /*Measure point to line distance in 2D space, point p is in inhomogeneous coordinates*/
    template <typename T> 
    inline T i_line_to_point_distance_2d(const T l[3], const T p[2])
    {
        return i_div(i_abs(i_dot2(l, p) + l[2]), i_l2_norm2(l));
    }

    /*Measure point to line distance in 3D space, point p is in inhomogeneous coordinates. 
      l is stored as 6-vector [a, b, c, x0, y0, z0]. Here [a, b, c] is the directional vector,
      but here we do not assume [a,b,c] is a unit vector.*/
    template <typename T> inline T i_line_to_point_distance_3d(const T l[6], const T p[3])
    {
        T pc[3], pcxl[3];
        i_sub3(p, l+3, pc);
        i_cross(pc, l, pcxl);
        return i_div(i_l2_norm3(pcxl), i_l2_norm3(l));
    }

    /*Fit a line l: ax+by+c = 0 in 2D space using total least square method. The input n 2D points in inhomogeneous
      coordinates. Array x has size 2*n and points are stored as [x0, y0, x1, y1, ...]. 
      x will be destroyed after calling this routine.*/ 
    template <typename T> inline void i_line_fit_2d_total_least_square(T* x, T l[3], int n)
    {
        i_zero3(l);
        if (n < 2)
        {
            return;
        }
        T ma[4], eigv[2], mq[4];
        /*compute the centroid of input data points*/
        int i, length = 2 * n;
        T xm = (T)0.0;
        T ym = (T)0.0;
        for (i = 0; i < length; i += 2)
        {
            xm += x[i];
            ym += x[i+1];
        }
        xm = i_div(xm, n);
        ym = i_div(ym, n);
        for (i = 0; i < length; i += 2)
        {
            x[i]   -= xm;
            x[i+1] -= ym;
        }
        i_mult_AtA_nx2(x, ma, n);
        i_eig_symmetric_2x2_closed(ma, eigv, mq);
        l[0] = mq[1];
        l[1] = mq[3];
        l[2] = -xm*l[0] - ym*l[1];
    }

    /*Fit a line l: ax+by+c = 0 in 2D space using total least square method. The input n 2D points in inhomogeneous
    coordinates. Array x has size 2*n and points are stored as [x0, y0, x1, y1, ...].
    x and indices will be destroyed after calling this routine.*/
    template <typename T> inline void i_line_fit_2d_total_least_square(
        T* x, int* indices, T l[3], int m, int n/*sample_size*/)
    {
        i_zero3(l);
        if (n < 2 || n > m)
        {
            return;
        }
        
        i_indexed_shuffle(x, indices, m, 2, n); 
        
        T ma[4], eigv[2], mq[4];
        /*compute the centroid of input data points*/
        int i, length = 2 * n;
        T xm = (T)0.0;
        T ym = (T)0.0;
        for (i = 0; i < length; i += 2)
        {
            xm += x[i];
            ym += x[i + 1];
        }
        xm /= n;
        ym /= n;
        for (i = 0; i < length; i += 2)
        {
            x[i] -= xm;
            x[i + 1] -= ym;
        }
        i_mult_AtA_nx2(x, ma, n);
        i_eig_symmetric_2x2_closed(ma, eigv, mq);
        l[0] = mq[1];
        l[1] = mq[3];
        l[2] = -xm*l[0] - ym*l[1];
    }

    /*Fit a line l: ax+by+c = 0 in 2D space with two 2D points in inhomogeneous space.*/
    template <typename T> inline void i_line_fit_2d(const T x[2], const T xp[2], T l[3])
    {
        T ma[4] = {x[0], x[1], xp[0], xp[1]};
        i_line_fit_2d_total_least_square(ma, l, 2);
    }

    /*Fit a line l: 
    x = at + x0; 
    y = bt + y0; 
    z = ct + z0; 
    (where [x0,y0,z0] is a point on the line and [a,b,c] is a unit vector) in 3D space using 
    total least square method (Orthogonal Distance Regression). The input n 3D points are in 
    inhomogeneous coordinates. Array x has size n*3 and points are stored as [x0, y0, z0, x1, y1, z1, ...].
    l is stored as 6-vector [a, b, c, x0, y0, z0]. x will be destroyed after calling this routine.*/
    template <typename T> inline void i_line_fit_3d_total_least_square(T* X, T l[6], int n)
    {
        i_zero6(l);
        if (n < 2)
        {
            return;
        }
        int i, j;
        T ma[9], eigv[3], mq[9];
        /*compute the centroid of input data points*/
        T xm = (T)0.0;
        T ym = (T)0.0;
        T zm = (T)0.0;
        for (i = 0, j = 0; i < n; i++)
        {
            xm += X[j++];
            ym += X[j++];
            zm += X[j++];
        }
        l[3] = xm = (xm / n);
        l[4] = ym = (ym / n);
        l[5] = zm = (zm / n);
        for (i = 0, j = 0; i < n; i++)
        {
            X[j++] -= xm;
            X[j++] -= ym;
            X[j++] -= zm;
        }
        i_mult_AtA_nx3(X, ma, n);
        i_eig_symmetric_3x3_closed(ma, eigv, mq);
        /*l's normal vector is the eigen vector of
          ma corresponding to its largest eigen value*/
        l[0] = mq[0];
        l[1] = mq[3];
        l[2] = mq[6];
        i_unitize3(l);
    }

    /*Fit a line l:
    x = at + x0;
    y = bt + y0;
    z = ct + z0;
    in 3D space with two 3D points in inhomogeneous space - minimal solution.*/
    template <typename T> inline void i_line_fit_3d(const T X[3], const T Xp[3], T l[6])
    {
        T ma[6] = {X[0], X[1], X[2], Xp[0], Xp[1], Xp[2]};
        i_line_fit_3d_total_least_square(ma, l, 2);
    }

}/* namespace idl */

#endif